///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2016, STEREOLABS.
//
// All rights reserved.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////




/***************************************************************************************************
 ** This sample demonstrates how to grab images and depth/disparity map with the ZED SDK          **
 ** Both images and depth/disparity map are displayed with OpenCV                                 **
 ** Most of the functions of the ZED SDK are linked with a key press event (using OpenCV)         **
 ***************************************************************************************************/


#include <stdio.h>
#include <string.h>
#include <ctime>
#include <chrono>
#include <iostream>

#include <opencv2/opencv.hpp>

#include <zed/Camera.hpp>
#include <zed/utils/GlobalDefine.hpp>

using namespace std;
using namespace sl::zed;


int main(int argc, char **argv) {


    string save_path = argv[1];
    string left_name = "/left";
    string right_name = "/right";
    string format_name = ".png";
    string left_write_name;
    string right_write_name;
    int img_num = 0;

    cout<< argc <<endl;
    sl::zed::Camera* zed;
    zed = new sl::zed::Camera(sl::zed::HD720);


    // init computation mode of the zed
    InitParams parameters;
    parameters.unit = MILLIMETER; // this sample is designed to work in METER
    parameters.verbose = true;
    parameters.mode = QUALITY;
    //parameters.disableSelfCalib = true;

    ERRCODE err = zed->init(parameters);

    std::cout << "Error code : " << sl::zed::errcode2str(err) << std::endl;
    if (err != sl::zed::SUCCESS) {
        // Exit if an error occurred
        delete zed;
        return 1;
    }

    char key = ' ';
    int viewID = 0;
    int confidenceThres = 100;


    zed->setDepthClampValue(10000);

	int wnd_flag = cv::WINDOW_AUTOSIZE /*| cv::WINDOW_OPENGL*/;
	cv::namedWindow("VIEW", wnd_flag);

    std::cout << "Press 'q' to exit" << std::endl;

    sl::zed::SENSING_MODE dm_type = sl::zed::FILL;
    // Jetson only. Execute the calling thread on core 2
    sl::zed::Camera::sticktoCPUCore(2);
    cv::Mat left_img, right_img;

    // Loop until 'q' is pressed
    while (key != 'q') {
        bool res = zed->grab(dm_type,1, 1, 0);

        if (!res) {
            slMat2cvMat(zed->retrieveImage(sl::zed::SIDE::LEFT_UNRECTIFIED)).copyTo(left_img);
            slMat2cvMat(zed->retrieveImage(sl::zed::SIDE::RIGHT_UNRECTIFIED)).copyTo(right_img);
            //depth = zed->retrieveMeasure(sl::zed::MEASURE::DEPTH); // Get the pointer
            //slMat2cvMat(zed->normalizeMeasure(sl::zed::MEASURE::DISPARITY)).copyTo(disp);
            imshow("LEFT", left_img);
            imshow("RIGHT", right_img);

            key = cv::waitKey(1);

            // Keyboard shortcuts
            switch (key) {
                case 'S':
                case 's':
                    left_write_name = save_path+left_name+to_string(img_num)+format_name;
                    right_write_name = save_path+right_name+to_string(img_num)+format_name;
                    imwrite(left_write_name.c_str(), left_img);
                    imwrite(right_write_name.c_str(), right_img);
                    img_num++;
                    cout<< left_write_name + " and " + right_write_name +" have been written"<<endl;
                    break;
                default:
                    break;
            }
        }
    }
    delete zed;
    return 0;
}
